#include "algo_filter.h"

// 带阻滤波器 30~98HZ
butterworth_parameter_t g_bandstop_filter_parameter_30_98={
	// 200hz---30hz-98hz  采样-阻带
	1,   0.627040f,  -0.290527f,
	0.354737f,   0.627040f,    0.354737f
};

// 带阻滤波器 30~94HZ
butterworth_parameter_t g_bandstop_filter_parameter_30_94={
	// 200hz---30hz-94hz  采样-阻带
	1,   0.5334540355829,  -0.2235264828971,
	0.3882367585514,   0.5334540355829,   0.3882367585514
};

// 陷波滤波器
butterworth_parameter_t g_notch_filter_parameter_55hz={
	// 200hz---55hz-5hz  采样-陷波频率-带宽
	1,   0.3008218748187,   0.9229897627825,
	0.9614948813912,   0.3008218748187,   0.9614948813912
};


// 自定义滤波器参数结构体
// 设定截止频率
// LPF：butterworth低通滤波器结构体指针
// sample_frequent：采样频率
// cutoff_frequent：截至频率
void set_cutoff_frequency(butterworth_parameter_t *LPF, f32 sample_frequent, f32 cutoff_frequent)
{
	f32 fr = sample_frequent / cutoff_frequent;
	f32 ohm = tanf(PI / fr);
	f32 c = 1.0f + 2.0f * cosf(PI / 4.0f) * ohm + ohm * ohm;
	
	if (cutoff_frequent <= 0.0f) {
	// no filtering
	return;
	}
	
	LPF->b[0] = ohm * ohm / c;
	LPF->b[1] = 2.0f * LPF->b[0];
	LPF->b[2] = LPF->b[0];
	LPF->a[0]=1.0f;
	LPF->a[1] = 2.0f * (ohm * ohm - 1.0f) / c;
	LPF->a[2] = (1.0f - 2.0f * cosf(PI / 4.0f) * ohm + ohm * ohm) / c;
}

// Butterworth低通滤波器
// parameter：滤波器参数
// buffer：滤波器缓冲区
// curr_input：当前输入
f32 butterworth_lowpass_filter(butterworth_parameter_t *parameter, butterworth_buffer_t *buffer, f32 curr_input)
{
	/* 获取最新x(n) */
	buffer->Input_Butter[2]=curr_input;
	/* Butterworth滤波 */
	buffer->Output_Butter[2]=
	parameter->b[0] * buffer->Input_Butter[2]
	  +parameter->b[1] * buffer->Input_Butter[1]
		+parameter->b[2] * buffer->Input_Butter[0]
		  -parameter->a[1] * buffer->Output_Butter[1]
			-parameter->a[2] * buffer->Output_Butter[0];
	/* x(n) 序列保存 */
	buffer->Input_Butter[0]=buffer->Input_Butter[1];
	buffer->Input_Butter[1]=buffer->Input_Butter[2];
	/* y(n) 序列保存 */
	buffer->Output_Butter[0]=buffer->Output_Butter[1];
	buffer->Output_Butter[1]=buffer->Output_Butter[2];
	return buffer->Output_Butter[2];
}
